/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SpeedController;
//import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;


public class FlywheelPID extends velocity_PIDSubsystem {
    private static final double Kp = 0.00001; //0.000004
    private static final double Ki = 0.0; //0.0000000125;
    private static final double Kd = 0.0003; //0.0005 took about 5 seconds; //0.0004;

    private SpeedController rightmotor = new Jaguar(RobotMap.flywheelMotorRight);
    private SpeedController leftmotor = new Jaguar(RobotMap.flywheelMotorLeft);
    private Counter hall = new Counter(RobotMap.hallsensor);
    //private Timer timer = new Timer();
    private double flyrate;
    private double hallperiod;
    private double oldflyrate;
    //private NetworkTable preferencesTable;

    // Initialize your subsystem here
    public FlywheelPID() {
        super("FlywheelPID", Kp, Ki, Kd);
        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.

        //set starting flyrate speed - this may not be necessary
        flyrate = 0.0;
        setSetpoint(flyrate);

        //set allowable range of flywheel speeds - this may not be necessary
        setSetpointRange(0.0, 3800.0);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(null);
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;

        /*
        hallperiod = hall.getPeriod();
        if (hallperiod > 0.01 && hallperiod < 0.25){
            flyrate = 60.0 / hallperiod;
            //System.out.println("hallperiod = " + hallperiod);
        }
        //Show current speed on dashboard
        SmartDashboard.putDouble("Current Flywheel RPM", flyrate);
         *
         */

        /*THE OLD SPEED AVERAGING ALGORITHM (BEFORE GETPERIOD WAS WORKING)
        if (timer.get() >= 3.0){
            flyrate = hall.get() / timer.get() * 60.0; //yields rpm
            hall.reset();
            timer.reset();
        }
         *
         */

        readhallspeed();
        //return the current speed to the PID loop
        return flyrate; //in revolutions per minute
    }

    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        setFlyspeed(output);
    }

    public void zerohall(){
        hall.start();
        hall.reset();
        //timer.start();
        //timer.reset();
    }

    public void zerospeed(){
        flyrate = 0.0;
    }

    public void setFlyspeed(double flyspeed){
        rightmotor.set(-flyspeed);
        leftmotor.set(-flyspeed);
        SmartDashboard.putDouble("Flywheel Power (%) = ", flyspeed * 100);
    }

    public void readhallspeed(){

        //The getPeriod() method in the counter class works very inconsistently
        //(it often reads "Infinity" instead of an actual number). This only
        //calculates the speed when it is not Infinity.
        oldflyrate = flyrate;
        hallperiod = hall.getPeriod();
        if (hallperiod > 0.01 && hallperiod < 0.25){
            flyrate = 60.0 / hallperiod;
            //System.out.println("hallperiod = " + hallperiod);
            //hall.reset();
            //timer.reset();
        }
        else{
            //keep the previous flyrate when getPeriod() is Infinity
            flyrate = oldflyrate;
        /*
        //SPEED AVERAGING ALGORITHM for when getPeriod is reading Infinity
        //check the rate of the flywheel every X seconds
            if (timer.get() >= 0.5){
                flyrate = hall.get() / timer.get() * 60; //yields rpm
                hall.reset();
                timer.reset();
            }
         */
        }

        SmartDashboard.putDouble("Current Flywheel RPM", flyrate);

    }

}
